/* -------------------------------------------------- 

Lighthouse3D 

VSML - Very Simple Matrix Library 

http://www.lighthouse3d.com/very-simple-libs 

----------------------------------------------------*/  

#include "vsml.h"  
#include <math.h>  

// This var keeps track of the single instance of VSML  
VSML* VSML::gInstance = 0;  

#define M_PI       3.14159265358979323846f  

static inline float  
DegToRad(float degrees)  
{  
	return (float)(degrees * (M_PI / 180.0f));  
};  

// Singleton implementation  
// use this function to get the instance of VSML  
VSML*  
VSML::getInstance (void) {  

	if (0 != gInstance)  
		return gInstance;  
	else  
		gInstance = new VSML();  

	return gInstance;  
}  

// VSML constructor  
VSML::VSML():  
mInit(false),  
mBlocks(false)  
{  

}  

// VSML destructor  
VSML::~VSML()  
{  
}  

// send the buffer data and offsets to VSML  
void  
VSML::initUniformBlock(GLuint buffer, GLuint modelviewOffset, GLuint projOffset)  
{  
	mInit = true;  
	mBlocks = true;  
	mBuffer = buffer;  
	mOffset[MODELVIEW] = modelviewOffset;  
	mOffset[PROJECTION] = projOffset;  
}  

// send the uniform locations to VSML  
void  
VSML::initUniformLocs(GLuint modelviewLoc, GLuint projLoc)  
{  
	mInit = true;  
	mBlocks = false;  
	mUniformLoc[MODELVIEW] = modelviewLoc;  
	mUniformLoc[PROJECTION] = projLoc;  
}  

// glPushMatrix implementation  
void  
VSML::pushMatrix(MatrixTypes aType) {  

	float *aux = (float *)malloc(sizeof(float) * 16);  
	memcpy(aux, mMatrix[aType], sizeof(float) * 16);  
	mMatrixStack[aType].push_back(aux);  
}  

// glPopMatrix implementation  
void  
VSML::popMatrix(MatrixTypes aType) {  

	float *m = mMatrixStack[aType][mMatrixStack[aType].size()-1];  
	memcpy(mMatrix[aType], m, sizeof(float) * 16);  
	mMatrixStack[aType].pop_back();  
	free(m);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(aType);  
#endif  

}  

// glLoadIdentity implementation  
void  
VSML::loadIdentity(MatrixTypes aType)  
{  
	setIdentityMatrix(mMatrix[aType]);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(aType);  
#endif  
}  

// glMultMatrix implementation  
void  
VSML::multMatrix(MatrixTypes aType, float *aMatrix)  
{  

	float *a, *b, res[16];  
	a = mMatrix[aType];  
	b = aMatrix;  

	for (int i = 0; i < 4; ++i) {  
		for (int j = 0; j < 4; ++j) {  
			res[j*4 + i] = 0.0f;  
			for (int k = 0; k < 4; ++k) {  
				res[j*4 + i] += a[k*4 + i] * b[j*4 + k];  
			}  
		}  
	}  
	memcpy(mMatrix[aType], res, 16 * sizeof(float));  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(aType);  
#endif  
}  

// glLoadMatrix implementation  
void  
VSML::loadMatrix(MatrixTypes aType, float *aMatrix)  
{  
	memcpy(mMatrix[aType], aMatrix, 16 * sizeof(float));  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(aType);  
#endif  
}  

// glTranslate implementation with matrix selection  
void  
VSML::translate(MatrixTypes aType, float x, float y, float z)  
{  
	float mat[16];  

	setIdentityMatrix(mat);  
	mat[12] = x;  
	mat[13] = y;  
	mat[14] = z;  

	multMatrix(aType,mat);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(aType);  
#endif  
}  

// glTranslate on the MODELVIEW matrix  
void  
VSML::translate(float x, float y, float z)  
{  
	translate(MODELVIEW, x,y,z);  
}  

// glScale implementation with matrix selection  
void  
VSML::scale(MatrixTypes aType, float x, float y, float z)  
{  
	float mat[16];  

	setIdentityMatrix(mat,4);  
	mat[0] = x;  
	mat[5] = y;  
	mat[10] = z;  

	multMatrix(aType,mat);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(aType);  
#endif  
}  

// glScale on the MODELVIEW matrix  
void  
VSML::scale(float x, float y, float z)  
{  
	scale(MODELVIEW, x, y, z);  
}  

// glRotate implementation with matrix selection  
void  
VSML::rotate(MatrixTypes aType, float angle, float x, float y, float z)  
{  
	float mat[16];  

	float radAngle = DegToRad(angle);  
	float co = cos(radAngle);  
	float si = sin(radAngle);  
	float x2 = x*x;  
	float y2 = y*y;  
	float z2 = z*z;  

	mat[0] = x2 + (y2 + z2) * co;  
	mat[4] = x * y * (1 - co) - z * si;  
	mat[8] = x * z * (1 - co) + y * si;  
	mat[12]= 0.0f;  

	mat[1] = x * y * (1 - co) + z * si;  
	mat[5] = y2 + (x2 + z2) * co;  
	mat[9] = y * z * (1 - co) - x * si;  
	mat[13]= 0.0f;  

	mat[2] = x * z * (1 - co) - y * si;  
	mat[6] = y * z * (1 - co) + x * si;  
	mat[10]= z2 + (x2 + y2) * co;  
	mat[14]= 0.0f;  

	mat[3] = 0.0f;  
	mat[7] = 0.0f;  
	mat[11]= 0.0f;  
	mat[15]= 1.0f;  

	multMatrix(aType,mat);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(aType);  
#endif  
}  

// glRotate implementation in the MODELVIEW matrix  
void  
VSML::rotate(float angle, float x, float y, float z)  
{  
	rotate(MODELVIEW,angle,x,y,z);  
}  

// gluLookAt implementation  
void  
VSML::lookAt(float xPos, float yPos, float zPos,  
			 float xLook, float yLook, float zLook,  
			 float xUp, float yUp, float zUp)  
{  
	float dir[3], right[3], up[3];  

	up[0] = xUp;    up[1] = yUp;    up[2] = zUp;  

	dir[0] =  (xLook - xPos);  
	dir[1] =  (yLook - yPos);  
	dir[2] =  (zLook - zPos);  
	normalize(dir);  

	crossProduct(dir,up,right);  
	normalize(right);  

	crossProduct(right,dir,up);  
	normalize(up);  

	float m1[16],m2[16];  

	m1[0]  = right[0];  
	m1[4]  = right[1];  
	m1[8]  = right[2];  
	m1[12] = 0.0f;  

	m1[1]  = up[0];  
	m1[5]  = up[1];  
	m1[9]  = up[2];  
	m1[13] = 0.0f;  

	m1[2]  = -dir[0];  
	m1[6]  = -dir[1];  
	m1[10] = -dir[2];  
	m1[14] =  0.0f;  

	m1[3]  = 0.0f;  
	m1[7]  = 0.0f;  
	m1[11] = 0.0f;  
	m1[15] = 1.0f;  

	setIdentityMatrix(m2,4);  
	m2[12] = -xPos;  
	m2[13] = -yPos;  
	m2[14] = -zPos;  

	multMatrix(MODELVIEW, m1);  
	multMatrix(MODELVIEW, m2);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(MODELVIEW);  
#endif  
}  

// gluPerspective implementation  
void  
VSML::perspective(float fov, float ratio, float nearp, float farp)  
{  
	float projMatrix[16];  

	float f = 1.0f / tan (fov * (M_PI / 360.0f));  

	setIdentityMatrix(projMatrix,4);  

	projMatrix[0] = f / ratio;  
	projMatrix[1 * 4 + 1] = f;  
	projMatrix[2 * 4 + 2] = (farp + nearp) / (nearp - farp);  
	projMatrix[3 * 4 + 2] = (2.0f * farp * nearp) / (nearp - farp);  
	projMatrix[2 * 4 + 3] = -1.0f;  
	projMatrix[3 * 4 + 3] = 0.0f;  

	multMatrix(PROJECTION, projMatrix);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(PROJECTION);  
#endif  
}  

// glOrtho implementation  
void  
VSML::ortho(float left, float right, float bottom, float top, float nearp, float farp)  
{  
	float m[16];  

	setIdentityMatrix(m,4);  

	m[0 * 4 + 0] = 2 / (right - left);  
	m[1 * 4 + 1] = 2 / (top - bottom);  
	m[2 * 4 + 2] = -2 / (farp - nearp);  
	m[3 * 4 + 0] = -(right + left) / (right - left);  
	m[3 * 4 + 1] = -(top + bottom) / (top - bottom);  
	m[3 * 4 + 2] = -(farp + nearp) / (farp - nearp);  

	multMatrix(PROJECTION, m);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(PROJECTION);  
#endif  
}  

// glFrustum implementation  
void  
VSML::frustum(float left, float right, float bottom, float top, float nearp, float farp)  
{  
	float m[16];  

	setIdentityMatrix(m,4);  

	m[0 * 4 + 0] = 2 * nearp / (right-left);  
	m[1 * 4 + 1] = 2 * nearp / (top - bottom);  
	m[2 * 4 + 0] = (right + left) / (right - left);  
	m[2 * 4 + 1] = (top + bottom) / (top - bottom);  
	m[2 * 4 + 2] = - (farp + nearp) / (farp - nearp);  
	m[2 * 4 + 3] = -1.0f;  
	m[3 * 4 + 2] = - 2 * farp * nearp / (farp-nearp);  
	m[3 * 4 + 3] = 0.0f;  

	multMatrix(PROJECTION, m);  

#ifdef VSML_ALWAYS_SEND_TO_OPENGL  
	matrixToGL(PROJECTION);  
#endif  
}  

// returns a pointer to the requested matrix  
float *  
VSML::get(MatrixTypes aType)  
{  
	return mMatrix[aType];  
}  

/* ----------------------------------------------------- 
SEND MATRICES TO OPENGL 
------------------------------------------------------*/  

// to be used with uniform blocks  
void  
VSML::matrixToBuffer(MatrixTypes aType)  
{  
	if (mInit && mBlocks) {  
		glBindBuffer(GL_UNIFORM_BUFFER,mBuffer);  
		glBufferSubData(GL_UNIFORM_BUFFER, mOffset[aType], 16 * sizeof(float), mMatrix[aType]);  
		glBindBuffer(GL_UNIFORM_BUFFER,0);  

	}  
}  

// to be used with uniform variables  
void  
VSML::matrixToUniform(MatrixTypes aType)  
{  
	if (mInit && !mBlocks) {  

		glUniformMatrix4fv(mUniformLoc[aType], 1, false, mMatrix[aType]);  
	}  
}  

// universal  
void  
VSML::matrixToGL(MatrixTypes aType)  
{  
	if (mInit) {  

		if (mBlocks) {  
			glBindBuffer(GL_UNIFORM_BUFFER,mBuffer);  
			glBufferSubData(GL_UNIFORM_BUFFER, mOffset[aType], 16 * sizeof(float), mMatrix[aType]);  
			glBindBuffer(GL_UNIFORM_BUFFER,0);  
		}  
		else {  
			glUniformMatrix4fv(mUniformLoc[aType], 1, false, mMatrix[aType]);  
		}  

	}  
}  

// -----------------------------------------------------  
//                      AUX functions  
// -----------------------------------------------------  

// sets the square matrix mat to the identity matrix,  
// size refers to the number of rows (or columns)  
void  
VSML::setIdentityMatrix( float *mat, int size) {  

	// fill matrix with 0s  
	for (int i = 0; i < size * size; ++i)  
		mat[i] = 0.0f;  

	// fill diagonal with 1s  
	for (int i = 0; i < size; ++i)  
		mat[i + i * size] = 1.0f;  
}  

// res = a cross b;  
void  
VSML::crossProduct( float *a, float *b, float *res) {  

	res[0] = a[1] * b[2]  -  b[1] * a[2];  
	res[1] = a[2] * b[0]  -  b[2] * a[0];  
	res[2] = a[0] * b[1]  -  b[0] * a[1];  
}  

// Normalize a vec3  
void  
VSML::normalize(float *a) {  

	float mag = sqrt(a[0] * a[0]  +  a[1] * a[1]  +  a[2] * a[2]);  

	a[0] /= mag;
	a[1] /= mag;
	a[2] /= mag;
}  